#include "mainwindow.h"
#include "ui_mainwindow.h"
#include <QtNetwork>

#include "crc16.h"

#include <QDebug>

MainWindow::MainWindow(QWidget *parent) :
    QMainWindow(parent),
    ui(new Ui::MainWindow)
{
    ui->setupUi(this);

    hostAddr = QHostAddress("192.168.3.255");

    udpSocket = new QUdpSocket(this);

    m_timer = new QTimer(this);
    connect(m_timer, &QTimer::timeout, this, &MainWindow::timeoutCallback);
    m_autoMoveTimer = new QTimer(this);
    connect(m_autoMoveTimer, &QTimer::timeout, this, &MainWindow::timeoutAutoMoveCallback);

    ui->spinInterval->setRange(5,200);
    ui->spinInterval->setValue(10);
    ui->sliderInterval->setRange(5, 200);
    ui->sliderInterval->setValue(10);
    ui->sliderVelocity->setRange(-61, 61);
    connect(ui->sliderInterval, &QSlider::valueChanged, this, &MainWindow::onSliderInterval);

    ui->sliderAngleT->setRange(-90,90);
    ui->sliderAngleG->setRange(0,90);
    connect(ui->sliderAngleT, &QSlider::valueChanged, this, &MainWindow::onSliderAngleT);
    connect(ui->sliderAngleG, &QSlider::valueChanged, this, &MainWindow::onSliderAngleG);



    ui->editDevIPAddr->setText("192.168.4.1");

    connect(ui->btnSend,&QPushButton::clicked, this, &MainWindow::onBtnSend);
    //udpSocket->bind(38899, QUdpSocket::ShareAddress);
    udpSocket->bind(QHostAddress::AnyIPv4, 38899);
    connect(udpSocket, &QUdpSocket::readyRead, this, &MainWindow::receiveUdpData);

    tcpSocket = new QTcpSocket(this);
    tcpSocket->abort();
    connect(tcpSocket, &QTcpSocket::readyRead, this, &MainWindow::receiveTcpData);
    connect(tcpSocket, QOverload<QAbstractSocket::SocketError>::of(&QTcpSocket::error), this, &MainWindow::displayError);

    connect(ui->btnConnect, &QPushButton::clicked, this, &MainWindow::onBtnConnect);
    connect(ui->btnAuto, &QPushButton::clicked, this, &MainWindow::onBtnAuto);

    connect(ui->btnSearchIP, &QPushButton::clicked, this, &MainWindow::onBtnSearchIP);
    connect(ui->btnClear, &QPushButton::clicked, this, &MainWindow::onBtnClear);
    connect(ui->btnSearchID, &QPushButton::clicked, this, &MainWindow::onBtnSearchID);
    connect(ui->btnSetID, &QPushButton::clicked, this, &MainWindow::onBtnSetID);
    connect(ui->btnReadData, &QPushButton::clicked, this, &MainWindow::onBtnReadData);
    connect(ui->btnWriteData, &QPushButton::clicked, this, &MainWindow::onBtnWriteData);
    connect(ui->sliderVelocity, &QSlider::valueChanged, this, &MainWindow::onSliderVelocity);
    connect(ui->btnRunMode, &QPushButton::clicked, this, &MainWindow::onBtnRunMode);
    connect(ui->btnSpeedMode, &QPushButton::clicked, this, &MainWindow::onBtnSpeedMode);
}

MainWindow::~MainWindow()
{
    delete ui;
}

QString MainWindow::readIPAddress()
{
    QString ip_address;
    QList<QHostAddress> ipAddressesList = QNetworkInterface::allAddresses();
    for (int i = 0; i < ipAddressesList.size(); ++i)
    {
         //qDebug() << ipAddressesList.at(i);
        if (ipAddressesList.at(i) != QHostAddress::LocalHost &&  ipAddressesList.at(i).toIPv4Address())
        {
            ip_address = ipAddressesList.at(i).toString();
            qDebug()<<ip_address;  //debug
            //break;
        }
    }
    if (ip_address.isEmpty())
    {
        ip_address = QHostAddress(QHostAddress::LocalHost).toString();
    }
    return ip_address;
}

void MainWindow::onBtnSend()
{
    QByteArray datagram = "hello world!";


    qDebug() << hostAddr.toString();


        static uint16_t value;
        if (value!=700)
        {
            value = 700;
        }
        else {
            value = 400;
        }
        uint8_t data[64] = {0xaa, 0x55,0x08, 0x0a, 0x06, 0x01, 0x1e, 0x02, 0x27, 0xf4, 0x01};
        data[9] = (value>>0)&0xff;
        data[10] = (value>>8)&0xff;
        uint16_t crc;
        crc = crc16Calc(&data[2], 9);
        data[11] = (crc>>0)&0xff;
        data[12] = (crc>>8)&0xff;
        udpSocket->writeDatagram(reinterpret_cast<char*>(data),13,
                                 hostAddr, 8899);

}

void MainWindow::onBtnConnect()
{
    if (ui->btnConnect->text()=="connect")
    {
        tcpSocket->connectToHost(ui->editDevIPAddr->text(), 8899);
        if (tcpSocket->waitForConnected(1000))
        {
            ui->btnConnect->setText("disconnect");

        }
    }
    else
    {
        tcpSocket->disconnectFromHost();
        if (tcpSocket->state()==QAbstractSocket::UnconnectedState ||
                tcpSocket->waitForDisconnected(1000))
        {
            ui->btnConnect->setText("connect");
        }
    }
}

void MainWindow::onBtnClear()
{
    ui->txtDisplay->clear();
    readIPAddress();
}

void MainWindow::onBtnAuto()
{
    if (ui->btnAuto->text()=="start")
    {
        ui->btnAuto->setText("stop");
        m_autoMoveTimer->start(ui->sliderInterval->value());
    }
    else
    {
        ui->btnAuto->setText("start");
        m_autoMoveTimer->stop();
    }
}

void MainWindow::onBtnSearchIP()
{
    QList<QNetworkInterface> networkinterfaces = QNetworkInterface::allInterfaces();
    char data[] = "www.usr.cn";

    foreach (QNetworkInterface interface, networkinterfaces)
    {
        foreach (QNetworkAddressEntry entry, interface.addressEntries())
        {
            QHostAddress broadcastAddress = entry.broadcast();
            if (broadcastAddress != QHostAddress::Null &&
                entry.ip() != QHostAddress::LocalHost &&
                entry.ip().protocol() == QAbstractSocket::IPv4Protocol &&
                broadcastAddress != QHostAddress("169.254.255.255"))//169.254.255.255 DHCP故障时提供的临时地址，无效IP
            {
                qDebug() << broadcastAddress;
                udpSocket->writeDatagram(data,10, broadcastAddress, 48899);
            }
        }
    }

    //auto ahostAddr = QHostAddress("192.168.1.255");
   // QHostAddress ahostAddr("255.255.255.255");
    //char data[] = "www.usr.cn";
   // udpSocket->writeDatagram(data,10,
    //                         QHostAddress::Broadcast, 48899);
    //qDebug() << ahostAddr << data;
}



void MainWindow::onBtnSearchID()
{
    uint8_t data[64] = {0xaa, 0x55,0x08, 0x01, 0x06, 0x01, 0x31, 0xff, 0x00, 0x00, 0x00};
    uint16_t crc;
    if (ui->btnSearchID->text()=="Scan ID")
    {
        ui->btnSearchID->setText("Stop Scan");
        data[7] = 0xff;
    }
    else
    {
        ui->btnSearchID->setText("Scan ID");
        data[7] = 0x01;
    }

    crc = crc16Calc(&data[2], 9);
    data[11] = (crc>>0)&0xff;
    data[12] = (crc>>8)&0xff;

    hostAddr.setAddress(ui->editDevIPAddr->text());
    udpSocket->writeDatagram(reinterpret_cast<char*>(data),13,
                             hostAddr, 8899);
}

void MainWindow::onBtnSetID()
{
    //aa 55 06 03 04 01 01 28 01
    uint8_t data[64] = {0xaa, 0x55,0x06, 0x03, 0x04, 0x01, 0x1f, 0x05, 0x20, 0x00, 0x00};
    uint16_t crc;



    data[6] = ui->editOldID->text().toUShort() & 0xff;
    data[8] = ui->editNewID->text().toUShort() & 0xff;

    qDebug() << data[6] << data[8];

    crc = crc16Calc(&data[2], 7);
    data[9] = (crc>>0)&0xff;
    data[10] = (crc>>8)&0xff;

    hostAddr.setAddress(ui->editDevIPAddr->text());
    udpSocket->writeDatagram(reinterpret_cast<char*>(data),11,
                             hostAddr, 8899);
}

void MainWindow::onBtnReadData()
{
    uint8_t list2byte[] = {0x09, 0x0b, 0x10, 0x18, 0x1c, 0x1f, 0x2a, 0x2e, 0x30, 0x38, 0x3a, 0x3c, 0x45};
    //aa 55 06 02 04 01 01 28 01
    uint8_t data[64] = {0xaa, 0x55,0x06, 0x02, 0x04, 0x01, 0x1f, 0x05, 0x01, 0x00, 0x00};
    uint16_t crc;


    data[6] = ui->editOldID->text().toUShort() & 0xff;
    data[7] = ui->editDataAddr->text().toUShort() & 0xff;

    for (uint8_t i=0; i<sizeof (list2byte); i++)
    {
        if (data[7]==list2byte[i])
        {
            data[8] = 0x02;
            break;
        }
    }



    crc = crc16Calc(&data[2], 7);
    data[9] = (crc>>0)&0xff;
    data[10] = (crc>>8)&0xff;

    hostAddr.setAddress(ui->editDevIPAddr->text());
    udpSocket->writeDatagram(reinterpret_cast<char*>(data),11,
                             hostAddr, 8899);
}

void MainWindow::onBtnWriteData()
{
    //aa 55 06 03 04 01 01 28 01
    uint8_t list2byte[] = {0x09, 0x0b, 0x10, 0x18, 0x1c, 0x1f, 0x2a, 0x2e, 0x30, 0x38, 0x3a, 0x3c, 0x45};
    uint8_t data[64] = {0xaa, 0x55, 0x06, 0x03, 0x04, 0x01, 0x1f, 0x28, 0x01};
    uint16_t crc;
    uint8_t index;
    uint8_t i;


    index=6;
    data[index++] = ui->editOldID->text().toUShort() & 0xff;
    data[index++] = ui->editDataAddr->text().toUShort() & 0xff;

    for (i=0; i<sizeof(list2byte); i++)
    {
        if (data[7]==list2byte[i])
        {
            data[index++] = ui->editDataValue->text().toUShort() & 0xff;
            data[index++] = (ui->editDataValue->text().toUShort() >> 8) & 0xff;
            data[2] = 0x07;
            data[4] = 0x05;
            qDebug() << "2byte:" << i << data[7];
            break;
        }
    }
    if (i==sizeof(list2byte))
    {
        data[index++] = ui->editDataValue->text().toUShort() & 0xff;
        qDebug() << "1byte:" << sizeof(list2byte);
    }


    crc = crc16Calc(&data[2], index-2);
    qDebug() << "crc index:" << index;
    data[index++] = (crc>>0)&0xff;
    data[index++] = (crc>>8)&0xff;


    hostAddr.setAddress(ui->editDevIPAddr->text());
    udpSocket->writeDatagram(reinterpret_cast<char*>(data),index,
                             hostAddr, 8899);
}

void MainWindow::onBtnRunMode()
{
    //aa 55 06 03 04 01 01 28 01

    // aa 55 0f 03 04 01 16 37 00 03 07 01 16 21 00 00 00 00
    //                  0       1    2     3     4     5     6 id  7lock 8     9    10    11    12 id  13mode 14
    uint8_t data[64] = {0xaa, 0x55, 0x0c, 0x03, 0x04, 0x01, 0x16, 0x37, 0x00, 0x03, 0x04, 0x01, 0x16, 0x21, 0x00};//{0xaa, 0x55, 0x06, 0x03, 0x04, 0x01, 0x1f, 0x21, 0x01};
    uint16_t crc;

    if (ui->btnRunMode->text()=="Wheel Mode")
    {
        ui->btnRunMode->setText("Joint Mode");
        data[14] = 0x01;
    }
    else
    {
        ui->btnRunMode->setText("Wheel Mode");
        data[14] = 0x00;
    }

    data[6] = data[12] = ui->editOldID->text().toUShort() & 0xff;


    crc = crc16Calc(&data[2], data[2]+1);
    data[15] = (crc>>0)&0xff;
    data[16] = (crc>>8)&0xff;

    hostAddr.setAddress(ui->editDevIPAddr->text());
    udpSocket->writeDatagram(reinterpret_cast<char*>(data),data[2]+5,
                             hostAddr, 8899);
}

void MainWindow::onBtnSpeedMode()
{
    lockEnble(false);
    setRunMode(1);
    turnSide(0);
    timerState = 0;
    m_timer->start(5000);
}

void MainWindow::onSliderInterval(int ms)
{
    m_autoMoveTimer->setInterval(ms);
    ui->spinInterval->setValue(ms);
}

void MainWindow::onSliderAngleT(int angle)
{
    int16_t value;
    ui->labelAngleT->setText(tr("%1").arg(angle));

    value = static_cast<int16_t>(angle * 4096 / 360 + 2048);
    //qDebug() << value;

    uint16_t velocity = ui->editVelocity->text().toUShort();

    //aa 55 0b 03 09 01 1f 2a 00 09 00 00 00 00
    uint8_t data[64] = {0xaa, 0x55,0x0b, 0x03, 0x09, 0x01, 0x1f, 0x2a, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00};
    data[6] = ui->editOldID->text().toUShort() & 0xff;
    data[8] = (value>>0)&0xff;
    data[9] = (value>>8)&0xff;
    data[12] = (velocity>>0)&0xff;
    data[13] = (velocity>>8)&0xff;
    uint16_t crc;
    crc = crc16Calc(&data[2], 12);
    data[14] = (crc>>0)&0xff;
    data[15] = (crc>>8)&0xff;

    hostAddr.setAddress(ui->editDevIPAddr->text());
    udpSocket->writeDatagram(reinterpret_cast<char*>(data),16,
                             hostAddr, 8899);
}

void MainWindow::onSliderAngleG(int angle)
{
    int16_t value;
    ui->labelAngleG->setText(tr("%1").arg(angle));

    value = static_cast<int16_t>(2048 - angle * 4096 / 360);

    //aa 55 0b 03 09 01 1f 2a 00 09 00 00 00 00
    uint8_t data[64] = {0xaa, 0x55,0x0b, 0x03, 0x09, 0x01, 0x29, 0x2a, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00};
    data[6] = ui->editOldID->text().toUShort() & 0xff;
    data[8] = (value>>0)&0xff;
    data[9] = (value>>8)&0xff;
    uint16_t crc;
    crc = crc16Calc(&data[2], 12);
    data[14] = (crc>>0)&0xff;
    data[15] = (crc>>8)&0xff;

    hostAddr.setAddress(ui->editDevIPAddr->text());
    udpSocket->writeDatagram(reinterpret_cast<char*>(data),16,
                             hostAddr, 8899);
}

void MainWindow::onSliderVelocity(int velocity)
{
    int16_t value;
    ui->labelVelocity->setText(tr("%1").arg(velocity));

    value = static_cast<int16_t>(velocity*68);//rpm --> pulse/s

    //bit15 为方向标记为
    if (value<0)
    {
        //取反后加标记位
        value =  -value;
        value |= (1<<15);
    }


    uint8_t data[64] = {0xaa, 0x55,0x0b, 0x03, 0x09, 0x01, 0x1f, 0x2a, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00};
    data[6] = ui->editOldID->text().toUShort() & 0xff;
    data[12] = (value>>0)&0xff;
    data[13] = (value>>8)&0xff;
    uint16_t crc;
    crc = crc16Calc(&data[2], 12);
    data[14] = (crc>>0)&0xff;
    data[15] = (crc>>8)&0xff;

    hostAddr.setAddress(ui->editDevIPAddr->text());
    udpSocket->writeDatagram(reinterpret_cast<char*>(data),16,
                             hostAddr, 8899);
}

void MainWindow::autoSetAngle()
{
    uint8_t packet[256];
    uint16_t index = 0;
    uint16_t crc = 0;
    uint16_t value;
    double angle;
    static uint16_t t=0;
    static uint16_t T=200;
    static uint16_t count=0;

    count++;

    angle = sin(2*3.1415926/T*t)*90;
    value = static_cast<uint16_t>(angle * 4096 / 360 + 2048);
    //value = static_cast<uint16_t>(angle/0.29);

    if (t<T)
    {
        t++;
    }
    else
    {
        t = 0;
    }

        index = 0;
        packet[index++] = 0xaa;//header1
        packet[index++] = 0x55;//header2
        packet[index++] = 0x0c;//total length
        packet[index++] = 0x0a;// syncwrite cmd
        packet[index++] = 0x0a;//data length
        packet[index++] = 0x01;         // ax servo
        packet[index++] = 0x2a;         // goal position addr
        packet[index++] = 0x06;         // goal position register length
        packet[index++] =  ui->editOldID->text().toUShort() & 0xff;         // id
        packet[index++] = (value>>0)&0xff;         // lower byte
        packet[index++] = (value>>8)&0xff;         // upper byte
        packet[index++] = 0x00;
        packet[index++] = 0x00;
        packet[index++] = 0x00;
        packet[index++] = 0x00;
        crc = crc16Calc(&packet[2], index-2);
        packet[index++] = crc & 0xff;
        packet[index++] = (crc >> 8)&0xff;
        hostAddr.setAddress(ui->editDevIPAddr->text());
        udpSocket->writeDatagram(reinterpret_cast<char*>(packet),index,
                                 hostAddr, 8899);
}

void MainWindow::receiveUdpData()
{
    QByteArray datagram;
    QHostAddress remoteAddr;
    quint16 remotePort;
    QString strHex;
    while (udpSocket->hasPendingDatagrams())
    {

        datagram.resize(static_cast<int>(udpSocket->pendingDatagramSize()));
        udpSocket->readDatagram(datagram.data(), datagram.size(), &remoteAddr, &remotePort);

        if (static_cast<uint8_t>(datagram.at(0))==0xaa)
        {
            readPack(datagram);
            strHex = datagram.toHex(' ');
            ui->txtDisplay->append(strHex);
        }
        else
        {
            ui->txtDisplay->append(datagram);
            qDebug() << datagram << "remote:" << remoteAddr.toString() << remotePort;
            QString str = datagram;
            QStringList strList = str.split(",");
            if (strList.size()>=2)
            {
                devIPAddr = strList[0];
                auto devMac  = strList[1];
                //devName = strList[2];
            }

            ui->editDevName->setText(devName);
            ui->editDevIPAddr->setText(devIPAddr);
        }


    }
}

void MainWindow::newConnet()
{


}

void MainWindow::receiveTcpData()
{
    QByteArray data = tcpSocket->readAll();
    if (!data.isEmpty())
    {
        ui->txtDisplay->append(data);
//        QString str = "return\r\n";
//        for (int i=0; i<data.size(); i++)
//        {
//             str += QString("%1").asprintf("%02X ",data.at(i)&0xff);
//        }
//        str += "\r\n";
//        ui->txtDisplay->append(str);
    }
}

void MainWindow::displayError(QAbstractSocket::SocketError)
{
    tcpSocket->disconnectFromHost();
    ui->btnConnect->setText("connect");
    ui->txtDisplay->append(tr("failed to connect server:%1").arg(tcpSocket->errorString()));
}

void MainWindow::timeoutCallback()
{
    switch (timerState) {
    case 0:

        readPositon();
        m_timer->setInterval(100);
        timerState = 1;
        break;
    case 1:
        minPosition = position;
        m_timer->setInterval(5000);
        turnSide(1);
        timerState = 2;
        break;
    case 2:
        readPositon();
        torqueEnable(false);
        m_timer->setInterval(100);
        timerState = 3;
        break;
    case 3:
        m_timer->stop();
        maxPosition = position;
        setOffset(false);
        timerState = 0;
        break;
    }
}

void MainWindow::timeoutAutoMoveCallback()
{
    //autoSetAngle();
    int angle;
    static int T=100,t=0;
    t++;
    if (t==T) t = 0;
    angle = static_cast<int>(sin(2*3.1415926/T*t)*90) ;
    onSliderAngleT(angle);
}

void MainWindow::turnSide(uint8_t side)
{
    int16_t speed,value;
    uint16_t crc;

    speed = side == 0 ? -40 : 40;

    value = static_cast<int16_t>(speed*68);//rpm --> pulse/s


    //bit15 为方向标记为
    if (value<0)
    {
        //取反后加标记位
        value =  -value;
        value |= (1<<15);
    }


    uint8_t data2[64] = {0xaa, 0x55,0x0b, 0x03, 0x09, 0x01, 0x25, 0x2a, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00};
    data2[6] = 0xfe;
    data2[12] = (value>>0)&0xff;
    data2[13] = (value>>8)&0xff;
    crc = crc16Calc(&data2[2], 12);
    data2[14] = (crc>>0)&0xff;
    data2[15] = (crc>>8)&0xff;

    hostAddr.setAddress(ui->editDevIPAddr->text());
    udpSocket->writeDatagram(reinterpret_cast<char*>(data2),16,
                             hostAddr, 8899);
}

void MainWindow::readPositon()
{
    uint8_t data[64] = {0xaa, 0x55,0x06, 0x02, 0x04, 0x01, 0x1f, 0x05, 0x02, 0x00, 0x00};
    uint16_t crc;
    hostAddr.setAddress(ui->editDevIPAddr->text());

    data[6] = ui->editOldID->text().toUShort() & 0xff;
    data[7] = 0x38;
    crc = crc16Calc(&data[2], 7);
    data[9] = (crc>>0)&0xff;
    data[10] = (crc>>8)&0xff;
    udpSocket->writeDatagram(reinterpret_cast<char*>(data),11, hostAddr, 8899);
}

void MainWindow::torqueEnable(bool action)
{
    uint8_t data[64] = {0xaa, 0x55,0x06, 0x03, 0x04, 0x01, 0x1f, 0x28, 0x00, 0x00, 0x00};
    uint16_t crc;
    hostAddr.setAddress(ui->editDevIPAddr->text());

    data[6] = 0xfe;
    data[7] = 0x28;
    data[8] = action==true? 0x01 : 0x00;
    crc = crc16Calc(&data[2], 7);
    data[9] = (crc>>0)&0xff;
    data[10] = (crc>>8)&0xff;
    udpSocket->writeDatagram(reinterpret_cast<char*>(data),11, hostAddr, 8899);
}

void MainWindow::lockEnble(bool action)
{
    uint8_t data[64] = {0xaa, 0x55, 0x06, 0x03, 0x04, 0x01, 0x1f, 0x28, 0x01};
    uint16_t crc;

    data[6] = 0xfe;
    data[7] = 0x37;
    data[8] = action==true?0x01:0x00;

    crc = crc16Calc(&data[2], 7);
    data[9] = (crc>>0)&0xff;
    data[10] = (crc>>8)&0xff;

    hostAddr.setAddress(ui->editDevIPAddr->text());
    udpSocket->writeDatagram(reinterpret_cast<char*>(data),11,
                             hostAddr, 8899);
}

void MainWindow::setRunMode(uint8_t mode)
{
    uint8_t data[64] = {0xaa, 0x55, 0x06, 0x03, 0x04, 0x01, 0x1f, 0x28, 0x01};
    uint16_t crc;
    data[7] = 0x21;
    data[8] = mode;

    data[6] = 0xfe;

    crc = crc16Calc(&data[2], 7);
    data[9] = (crc>>0)&0xff;
    data[10] = (crc>>8)&0xff;

    hostAddr.setAddress(ui->editDevIPAddr->text());
    udpSocket->writeDatagram(reinterpret_cast<char*>(data),11,
                             hostAddr, 8899);
}

void MainWindow::setOffset(bool isInitial)
{
    int16_t offset;
    int16_t halfRange;

    if (isInitial==false)
    {
        if (maxPosition>minPosition)
        {
            halfRange = static_cast<int16_t>((maxPosition-minPosition)/2);
            offset = (halfRange+minPosition) - 2048;
        }
        else {
            halfRange = (4096-minPosition+maxPosition)/2;
            if (maxPosition>halfRange)
            {
                offset = (maxPosition - halfRange) - 2048;
            }
            else {
                offset = (minPosition + halfRange) - 2048;
            }
        }

        if (offset<0)
        {
            offset = -offset;
            offset |= (1<<11);
        }
    }
    else {
        offset = 0;
    }



    uint8_t data[64] = {0xaa, 0x55, 0x07, 0x03, 0x05, 0x01, 0x1f, 0x28, 0x01};
    uint16_t crc;
    uint8_t index;

    index=0;
    data[index++] = 0xaa;//header1
    data[index++] = 0x55;//header2
    data[index++] = 0x07;//total length
    data[index++] = 0x03;//write cmd
    data[index++] = 0x05;//data length
    data[index++] = 0x01;//servo type
    data[index++] = ui->editOldID->text().toUShort() & 0xff;
    data[index++] = 0x1f;//data address
    data[index++] = offset & 0xff;
    data[index++] = (offset >> 8) & 0xff;

    crc = crc16Calc(&data[2], index-2);
    data[index++] = (crc>>0)&0xff;
    data[index++] = (crc>>8)&0xff;


    hostAddr.setAddress(ui->editDevIPAddr->text());
    udpSocket->writeDatagram(reinterpret_cast<char*>(data),index,
                             hostAddr, 8899);
}

void MainWindow::readPack(QByteArray data)
{
    static uint8_t status=0;
    static uint8_t packet[128];
    static uint8_t j;
    static uint16_t crc;


    for (uint8_t i=0; i<data.size(); i++)
    {
        switch (status)
        {
        case 0://header0 0xaa
            if (static_cast<uint8_t>(data.at(i))==0xaa)
            {
                status = 1;
            }
            break;
        case 1://header1 0x55
            if (static_cast<uint8_t>(data.at(i))==0x55)
            {
                status = 2;
            }
            else
            {
                status = 0;
            }
            break;
        case 2://length
            packet[0] = static_cast<uint8_t>(data.at(i));
            if (packet[0] < 3)
            {
                status = 0;
            }
            else {
                j = 0;
                status = 3;
            }
            break;
        case 3://data
            packet[j+1] = static_cast<uint8_t>(data.at(i));
            if (++j == packet[0])
            {
                status = 4;
            }
            break;
        case 4://lower byte of crc
            crc = static_cast<uint8_t>(data.at(i));
            status = 5;
            break;
        case 5://upper byte of crc
            crc |= (static_cast<uint8_t>(data.at(i)))<<8;
            if (crc16Calc(packet, packet[0]+1) == crc)
            {
                unpackData(packet);
            }
            status = 0;
            break;
        default:
            break;
        }
    }

}

void MainWindow::unpackData(uint8_t *data)
{
    uint8_t *sub;
    uint16_t len;
    uint8_t sub_header;
    uint8_t sub_len;
    uint8_t data_index;

    len = data[0];
    data_index = 1;

    while (len > 0)
    {
        sub = &data[data_index];
        sub_header = sub[0];
        sub_len    = sub[1];
        if (len>=(sub_len+2))
        {
            len -= (sub_len+2);
            data_index += (sub_len+2);
        }
        else {
            break;
        }
        //qDebug() << sub_header << sub_len;
        switch (sub_header) {
        case 1://ping result
            for (uint8_t i=0; i<(sub[1]-1)/2; i++)
            {
                ui->txtDisplay->append(tr("position:%1, id:%2").arg(sub[3+i*2]).arg(sub[4+i*2]));
            }
            break;
        case 2://read result
        {
            if (sub[4]==1)
            {
                uint8_t value = sub[6];
                ui->txtDisplay->append(tr("read: %1").arg(value));
            }
            else if(sub[4]==2) {
                uint16_t value = static_cast<uint16_t>(sub[6]|(sub[7]<<8));
                position = value;
                ui->txtDisplay->append(tr("read: %1").arg(value));
            }
        }
            break;
        case 3://error
            break;
        case 4://id setting result
            break;
        case 5:
            break;
        case 0xff://compile time
            break;
        }
    }
}
